﻿#ifndef __SVD_DOUBLE_PRECISION_C_PROGRAM

#define __SVD_DOUBLE_PRECISION_C_PROGRAM

using namespace std;

//#include "nrutil.h"

#include "svd_double.h"

//#define SIGN(a, b) ((b) >= 0.0 ? fabsl(a) : -fabsl(a))
//
//static long double maxarg1, maxarg2;
//
//#define FMAX(a, b) (maxarg1=(a),maxarg2=(b),(maxarg1) > (maxarg2) ?\
//
//(maxarg1) : (maxarg2))
//
//static int iminarg1, iminarg2;
//
//#define IMIN(a, b) (iminarg1=(a),iminarg2=(b),(iminarg1) < (iminarg2) ?\
//
//(iminarg1) : (iminarg2))
//
//static long double sqrarg;
//
//#define SQR(a) ((sqrarg = (a)) == 0.0 ? 0.0 : sqrarg * sqrarg)

// 使用普通函数替代 SIGN 宏
long double SIGN(long double a, long double b) {
    return (b >= 0.0) ? fabsl(a) : -fabsl(a);
}

// 使用普通函数替代 FMAX 宏
long double FMAX(long double a, long double b) {
    return (a > b) ? a : b;
}

// 使用普通函数替代 IMIN 宏
int IMIN(int a, int b) {
    return (a < b) ? a : b;
}

// 使用普通函数替代 SQR 宏
long double SQR(long double sqrarg) {
    return (sqrarg == 0.0) ? 0.0 : sqrarg * sqrarg;
}

//A=U*W*VT.

//Here we store a, w, and v as one-dimensional arrays.

//So a[i][j] of matrix m+1-by-n+1 should be expressed as a[i*(n+1)+j],

//and v[i][j] of matrix n+1-by-n+1 should be expressed as v[i*(n+1)+j].

//Also, Because the start index now is one but not zero in the original c,

//an m-by-n matrix A is stored in array a with (m+1)*(n+1) entries.

//Similarly, n-by-n matrix V is stored in array v with (n+1)*(n+1) entries.

//Also, w is an array with n+1 entries.

void svdcmp(double* a, int m, int n, double w[], double* v)

//Given a matrix a[1..m][1..n], this routine computes its singular value decomposition, A =

//U��W��V T. Thematrix U replaces a on output. The diagonal matrix of singular values W is output

//as a vector w[1..n]. The matrix V (not the transpose V T ) is output as v[1..n][1..n].

{

    double pythag(double a, double b);

    int flag, i, its, j, jj, k, l, nm, n1 = n + 1;

    double anorm, c, f, g, h, s, scale, x, y, z, * rv1;

    rv1 = new double[n + 1];

    g = scale = anorm = 0.0; //Householder reduction to bidiagonal form.

    for (i = 1; i <= n; i++) {

        l = i + 1;

        rv1[i] = scale * g;

        g = s = scale = 0.0;

        if (i <= m) {

            for (k = i; k <= m; k++) scale += fabs(a[k * n1 + i]);

            if (scale) {

                for (k = i; k <= m; k++) {

                    a[k * n1 + i] /= scale;

                    s += a[k * n1 + i] * a[k * n1 + i];

                }

                f = a[i * n1 + i];

                g = -SIGN(sqrt(s), f);

                h = f * g - s;

                a[i * n1 + i] = f - g;

                for (j = l; j <= n; j++) {

                    for (s = 0.0, k = i; k <= m; k++) s += a[k * n1 + i] * a[k * n1 + j];

                    f = s / h;

                    for (k = i; k <= m; k++) a[k * n1 + j] += f * a[k * n1 + i];

                }

                for (k = i; k <= m; k++) a[k * n1 + i] *= scale;

            }

        }

        w[i] = scale * g;

        g = s = scale = 0.0;

        if (i <= m && i != n) {

            for (k = l; k <= n; k++) scale += fabs(a[i * n1 + k]);

            if (scale) {

                for (k = l; k <= n; k++) {

                    a[i * n1 + k] /= scale;

                    s += a[i * n1 + k] * a[i * n1 + k];

                }

                f = a[i * n1 + l];

                g = -SIGN(sqrt(s), f);

                h = f * g - s;

                a[i * n1 + l] = f - g;

                for (k = l; k <= n; k++) rv1[k] = a[i * n1 + k] / h;

                for (j = l; j <= m; j++) {

                    for (s = 0.0, k = l; k <= n; k++) s += a[j * n1 + k] * a[i * n1 + k];

                    for (k = l; k <= n; k++) a[j * n1 + k] += s * rv1[k];

                }

                for (k = l; k <= n; k++) a[i * n1 + k] *= scale;

            }

        }

        anorm = FMAX(anorm, (fabs(w[i]) + fabs(rv1[i])));

    }

    for (i = n; i >= 1; i--) { //Accumulation of right-hand transformations.

        if (i < n) {

            if (g) {

                for (j = l; j <= n; j++) //Double division to avoid possible underflow.

                    v[j * n1 + i] = (a[i * n1 + j] / a[i * n1 + l]) / g;

                for (j = l; j <= n; j++) {

                    for (s = 0.0, k = l; k <= n; k++) s += a[i * n1 + k] * v[k * n1 + j];

                    for (k = l; k <= n; k++) v[k * n1 + j] += s * v[k * n1 + i];

                }

            }

            for (j = l; j <= n; j++) v[i * n1 + j] = v[j * n1 + i] = 0.0;

        }

        v[i * n1 + i] = 1.0;

        g = rv1[i];

        l = i;

    }

    for (i = IMIN(m, n); i >= 1; i--) { //Accumulation of left-hand transformations.

        l = i + 1;

        g = w[i];

        for (j = l; j <= n; j++) a[i * n1 + j] = 0.0;

        if (g) {

            g = 1.0 / g;

            for (j = l; j <= n; j++) {

                for (s = 0.0, k = l; k <= m; k++) s += a[k * n1 + i] * a[k * n1 + j];

                f = (s / a[i * n1 + i]) * g;

                for (k = i; k <= m; k++) a[k * n1 + j] += f * a[k * n1 + i];

            }

            for (j = i; j <= m; j++) a[j * n1 + i] *= g;

        }
        else for (j = i; j <= m; j++) a[j * n1 + i] = 0.0;

        ++a[i * n1 + i];

    }

    for (k = n; k >= 1; k--) { //Diagonalization of the bidiagonal form: Loop over

        //singular values, and over allowed iterations.

        for (its = 1; its <= 30; its++) {

            flag = 1;

            for (l = k; l >= 1; l--) { //Test for splitting.

                nm = l - 1; //Note that rv1[1] is always zero.

                if ((double)(fabs(rv1[l]) + anorm) == anorm) {

                    flag = 0;

                    break;

                }

                if ((double)(fabs(w[nm]) + anorm) == anorm) break;

            }

            if (flag) {

                c = 0.0; //Cancellation of rv1[l], if l > 1.

                s = 1.0;

                for (i = l; i <= k; i++) {

                    f = s * rv1[i];

                    rv1[i] = c * rv1[i];

                    if ((double)(fabs(f) + anorm) == anorm) break;

                    g = w[i];

                    h = pythag(f, g);

                    w[i] = h;

                    h = 1.0 / h;

                    c = g * h;

                    s = -f * h;

                    for (j = 1; j <= m; j++) {

                        y = a[j * n1 + nm];

                        z = a[j * n1 + i];

                        a[j * n1 + nm] = y * c + z * s;

                        a[j * n1 + i] = z * c - y * s;

                    }

                }

            }

            z = w[k];

            if (l == k) { //Convergence.

                if (z < 0.0) { //Singular value is made nonnegative.

                    w[k] = -z;

                    for (j = 1; j <= n; j++) v[j * n1 + k] = -v[j * n1 + k];

                }

                break;

            }

            if (its == 100) {

                cout << "\nno convergence in 100 svdcmp iterations." <<

                    "Input a character to terminate the program";

                char key;

                cin >> key;

                exit(0);
            }

            x = w[l]; //Shift from bottom 2-by-2 minor.

            nm = k - 1;

            y = w[nm];

            g = rv1[nm];

            h = rv1[k];

            f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);

            g = pythag(f, 1.0);

            f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;

            c = s = 1.0; //Next QR transformation:

            for (j = l; j <= nm; j++) {

                i = j + 1;

                g = rv1[i];

                y = w[i];

                h = s * g;

                g = c * g;

                z = pythag(f, h);

                rv1[j] = z;

                c = f / z;

                s = h / z;

                f = x * c + g * s;

                g = g * c - x * s;

                h = y * s;

                y *= c;

                for (jj = 1; jj <= n; jj++) {

                    x = v[jj * n1 + j];

                    z = v[jj * n1 + i];

                    v[jj * n1 + j] = x * c + z * s;

                    v[jj * n1 + i] = z * c - x * s;

                }

                z = pythag(f, h);

                w[j] = z; //Rotation can be arbitrary if z = 0.

                if (z) {

                    z = 1.0 / z;

                    c = f * z;

                    s = h * z;

                }

                f = c * g + s * y;

                x = c * y - s * g;

                for (jj = 1; jj <= m; jj++) {

                    y = a[jj * n1 + j];

                    z = a[jj * n1 + i];

                    a[jj * n1 + j] = y * c + z * s;

                    a[jj * n1 + i] = z * c - y * s;

                }

            }

            rv1[l] = 0.0;

            rv1[k] = f;

            w[k] = x;

        }

    }

    delete []rv1;

}


double pythag(double a, double b)

//Computes (a2 + b2)1/2 without destructive underflow or overflow.

{

    double absa, absb;

    absa = fabs(a);

    absb = fabs(b);

    if (absa > absb) return absa * sqrt(1.0 + SQR(absb / absa));

    else return (absb == 0.0 ? 0.0 : absb * sqrt(1.0 + SQR(absa / absb)));

}

#endif



